#include "ImageConsProd.h"
#include "AngleSolver.h"
#include "RMVideoCapture.h"

using namespace std;

#define VIDEO_WIDTH  640
#define VIDEO_HEIGHT 360
#define BUFFER_SIZE 1
volatile unsigned int prdIdx;
volatile unsigned int csmIdx;
struct ImageData {
    Mat img;
    unsigned int frame;
};

ImageData data[BUFFER_SIZE];


void ImageConsProd::ImageProducer()
{
    RMVideoCapture cap("/dev/video1", 3);
    cap.setVideoFormat(VIDEO_WIDTH, VIDEO_HEIGHT, 1);
    cap.setExposureTime(0, 20);//settings->exposure_time);
    cap.startStream();
    cap.info();
    while(1)
    {
        while(prdIdx - csmIdx >= BUFFER_SIZE);
        cap >> data[prdIdx % BUFFER_SIZE].img;
        data[prdIdx % BUFFER_SIZE].frame = cap.getFrameCount();
        ++prdIdx;
    }
}

void ImageConsProd::ImageConsumer()
{
    Settings & setting = *settings;

    // load calibration parameter
    FileStorage fs(setting.intrinsic_file_480, FileStorage::READ);
    if (!fs.isOpened())	{
        cout << "Could not open the configuration file: \"" << setting.intrinsic_file_480 << "\"" << endl;
        return ;
    }
    Mat cam_matrix_480, distortion_coeff_480;
    fs["Camera_Matrix"] >> cam_matrix_480;
    fs["Distortion_Coefficients"] >> distortion_coeff_480;
    cout << cam_matrix_480 << endl;
    cout << distortion_coeff_480 << endl;

    AngleSolver solver_480(cam_matrix_480, distortion_coeff_480, 21.6, 5.4
                           , settings->scale_z_480, settings->min_detect_distance, settings->max_detect_distance);

    Point2f image_center_480 = Point2f(cam_matrix_480.at<double>(0, 2)
                                     ,cam_matrix_480.at<double>(1, 2));
    // parameter of PTZ and barrel
    const double overlap_dist = 100000.0;
    const double barrel_ptz_offset_y = 3.3;
    const double ptz_camera_y = 0.8;
    const double ptz_camera_z = -20.5;
    double theta = -atan((ptz_camera_y + barrel_ptz_offset_y)/overlap_dist);
    double r_data[] = {1,0,0,0,cos(theta),-sin(theta),0,sin(theta),cos(theta)};
    double t_data[] = {0,ptz_camera_y,ptz_camera_z}; // ptz org position in camera coodinate system
    Mat t_camera_ptz(3, 1, CV_64FC1, t_data);
    Mat r_camera_ptz(3, 3, CV_64FC1, r_data);
    solver_480.setRelationPoseCameraPTZ(r_camera_ptz, t_camera_ptz, barrel_ptz_offset_y);

    AngleSolverFactory angle_solver;
    angle_solver.setTargetSize(21.6,5.4, AngleSolverFactory::TARGET_ARMOR);
    angle_solver.setTargetSize(12.4,5.4, AngleSolverFactory::TARGET_SMALL_ARMOR);

    // load armor detector template
    ArmorDetector armor_detector(setting.armor);
    Mat template_img = imread(setting.template_image_file);
    Mat small_template_img = imread(setting.small_template_image_file);
    armor_detector.initTemplate(template_img, small_template_img);
//    armor_detector.setPnPSlover(&solver_480, &solver_480);
    Mat src;
    int frame_num = 0;
    while(1){
        // waiting for image data ready
        while(prdIdx - csmIdx == 0);
        data[csmIdx % BUFFER_SIZE].img.copyTo(src);
        frame_num = data[csmIdx % BUFFER_SIZE].frame;
        ++csmIdx;

        RotatedRect rect;
        rect = armor_detector.getTargetArea(src);
        Point2f points[4];
        rect.points(points);
        for(int i = 0;i < 4; i++)
        {
            line(src,points[i], points[(i+1)%4], Scalar(0,255,255),2);
        }
        imshow("src", src);
        waitKey(1);
    }
}

